#include "xx.h"

using namespace UNITREE_LEGGED_SDK;
const int PC_PORT = 8887;
const char PC_IP_ADDR[] = "localhost";
const int UDP_BUFFER_SIZE = 128;
// Globals
bool RUNNING = true; // indicating whether the main loop is running

std::string movement;
double command_number = 0.0;
int done = 0;

class UdpReceiver
{
public:
	std::deque<std::string> msg_queue;

	/* udp receiver thread */
	void run()
	{
		/********************UDP_Receiving_Initializing********************/
		socklen_t fromlen;
		struct sockaddr_in server_recv;
		struct sockaddr_in hold_recv;

		int sock_recv = socket(AF_INET, SOCK_DGRAM, 0);

		int sock_length_recv = sizeof(server_recv);
		bzero(&server_recv, sock_length_recv);

		server_recv.sin_family = AF_INET;
		server_recv.sin_addr.s_addr = INADDR_ANY;
		server_recv.sin_port = htons(PC_PORT); // Setting port of this program
		bind(sock_recv, (struct sockaddr *)&server_recv, sock_length_recv);
		fromlen = sizeof(struct sockaddr_in);
		char buf_UDP_recv[UDP_BUFFER_SIZE]; // for holding UDP data
		/******************************************************************/
		while (RUNNING)
		{
            memset(buf_UDP_recv, 0, sizeof buf_UDP_recv);
			int datalength = recvfrom(sock_recv, buf_UDP_recv, UDP_BUFFER_SIZE, 0, (struct sockaddr *)&hold_recv, &fromlen);
            std::string str(buf_UDP_recv);
            msg_queue.push_back(buf_UDP_recv);
            while(msg_queue.size()>1){
                msg_queue.pop_front();
            }
            movement = msg_queue[0];
		}
	}
};
class Custom
{
public:
    Custom(): control(LeggedType::A1, HIGHLEVEL),udp(){control.InitCmdData(cmd);}
    void UDPRecv();
    void UDPSend();
    void RobotControl();

    Control control;
    UDP udp;
    HighCmd cmd = {0};
    HighState state = {0};
    int motiontime = 0;
};


void Custom::UDPRecv()
{
    udp.Recv();
}

void Custom::UDPSend()
{  
    udp.Send();
}

void Custom::RobotControl() 
{
    cmd.forwardSpeed = 0.0f;
    cmd.sideSpeed = 0.0f;
    cmd.rotateSpeed = 0.0f;
    cmd.bodyHeight = 0.0f;

    cmd.mode = 0;
    cmd.roll  = 0;
    cmd.pitch = 0;
    cmd.yaw = 0;

    // if(done) return;

    // motiontime += 2;
    motiontime++;
    udp.GetRecv(state);
     float x_pos = atof(movement.c_str());
    // std::cout << x_pos << std::endl;
    // printf("%d   %f\n", motiontime, state.forwardSpeed);

    if (x_pos == -1)
    {
        std::cout << x_pos << " not " << std::endl;
    }
    if(x_pos < -1)
    {
        done = 1;
        if(x_pos == -10)   // get down
        {
            cmd.mode = 2;
            cmd.bodyHeight = -0.5f;
        }
        else if(x_pos == -11)   // nod
        {
            cmd.mode = 2;
            cmd.pitch = -0.5;
            cmd.pitch = 0.4;
        }
        else if(x_pos == -12)   // turn left
        {
            cmd.mode = 2;
            cmd.rotateSpeed = 0.2f;
        }
        else if(x_pos == -13)   // turn right
        {
            cmd.mode = 2;
            cmd.rotateSpeed = -0.2f;
        }
        udp.SetSend(cmd);
        return;
    }
    
    else if(0 < int(x_pos) && int(x_pos) < 240)   // turn left
    {
        cmd.mode = 2;
        // cmd.rotateSpeed = 0.1f;
        cmd.sideSpeed = 0.1f;
        std::cout << x_pos << " left " << std::endl;
    }
    else if(int(x_pos) > 335)        // turn right
    {
        cmd.mode = 2;
        // cmd.rotateSpeed = -0.1f;
        cmd.sideSpeed = -0.1f;
        std::cout << x_pos << " right " << std::endl;
    }
    else                        // go ahead
    {
        cmd.mode = 2;
	    cmd.forwardSpeed = 0.1f;
        std::cout << x_pos << " go ahead " << std::endl;
    }

    udp.SetSend(cmd);
}

int main(void) 
{
    std::cout << "Case1 begin." << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    printf("%ld  %ld\n", sizeof(HighCmd), sizeof(HighState) );
    printf("%ld  %ld\n", sizeof(LowCmd), sizeof(LowState) );
    UdpReceiver udp_receiver = UdpReceiver();
    std::thread udp_recv_thread(&UdpReceiver::run,&udp_receiver); // run udpRecv in a separate thread
    Custom custom;

    LoopFunc controlTask("control_loop", 0.002, 3, boost::bind(&Custom::RobotControl, &custom));
    LoopFunc udpSendTask("udp_send", 0.002, 3, boost::bind(&Custom::UDPSend, &custom));
    LoopFunc udpRecvTask("udp_recv", 0.002, 3, boost::bind(&Custom::UDPRecv, &custom));

    udpSendTask.start();
    udpRecvTask.start();
    controlTask.start();
    while(1){
        sleep(10);
    };

    return 0; 
}
